//Debug data
#define DEBUG 1
#define BAUD 38400

// Define pin assignments
#define LEDPIN 13
#define THROTTLEPIN 2
#define ROLLPIN 3
#define PITCHPIN 4
#define YAWPIN 5

// Define transmitter commands
#define THROTTLETIMEOUT 25000
#define PULSETIMEOUT 20000
int throttle = 0;
int roll = 0;
int pitch = 0;
int yaw = 0;
unsigned long previousTime = 0;
unsigned long deltaTime = 0;

void setup() {
  pinMode(LEDPIN, OUTPUT);
  digitalWrite(LEDPIN, HIGH);
  pinMode(THROTTLEPIN, INPUT);
  pinMode(ROLLPIN, INPUT);
  pinMode(PITCHPIN, INPUT);
  pinMode(YAWPIN, INPUT);
  Serial.begin(BAUD);
}

void loop() {
  // Read transmitter commands
  previousTime = millis();
  throttle = pulseIn(THROTTLEPIN, HIGH, THROTTLETIMEOUT);
  roll = pulseIn(ROLLPIN, HIGH, PULSETIMEOUT);
  pitch = pulseIn(PITCHPIN, HIGH, PULSETIMEOUT);
  yaw = pulseIn(YAWPIN, HIGH, PULSETIMEOUT);
  deltaTime = millis() - previousTime;
  
  if (DEBUG) {
    digitalWrite(LEDPIN, HIGH);
    Serial.print(deltaTime);
    Serial.print(",");
    Serial.print(throttle);
    Serial.print(",");
    Serial.print(roll);
    Serial.print(",");
    Serial.print(pitch);
    Serial.print(",");
    Serial.println(yaw);
    digitalWrite(LEDPIN, LOW);  
  }  
}
